#!/usr/bin/env python3

import numpy as np
import tf.transformations as tf
import rospy
import moveit_commander
import sys

# 初始化ROS和MoveIt
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('matrix_conversion')

# 创建机器人对象
arm_group = moveit_commander.MoveGroupCommander("manipulator")

# 获取末端执行器的当前姿态
current_pose = arm_group.get_current_pose().pose

# 提取位置和四元数
position = current_pose.position
orientation = current_pose.orientation

# 将四元数转换为旋转矩阵
rotation_matrix = tf.quaternion_matrix([orientation.x, orientation.y, orientation.z, orientation.w])

# 创建4x4变换矩阵
transform_matrix = np.identity(4)
transform_matrix[:3, :3] = rotation_matrix[:3, :3]  # 旋转矩阵部分
transform_matrix[0, 3] = position.x  # x坐标
transform_matrix[1, 3] = position.y  # y坐标
transform_matrix[2, 3] = position.z  # z坐标

print("4x4 Transform Matrix:")
print(transform_matrix)
